#ifndef __APP_MOTION_H__
#define __APP_MOTION_H__

#include <basic.hpp>
#include <algorithm.hpp>

// #include "basic.h"

#define MAX_SUPPORT_MOTOR_NUM (2)
#define WHEEL_DIAMETER (0.064f)           /*meter*/
#define PULSE_DELTA_ANGLE (0.023710f)     /*rad*/
#define DETECT_MAX_ANGLE_VOLOCITY (89.5f) /*rad/s*/

typedef struct
{
    TIM_HandleTypeDef *tim;
    uint16_t channel;
} pwm_t;

extern "C" void user_motor_gpio_inturrupt_handler(uint16_t GPIO_Pin);
class Motor
{
public:
    typedef enum
    {
        MODE_FORWARD = 0,
        MODE_BACKWARD,
        MODE_NETURAL,
        MODE_STOP,
    } Motor_Mode;
    typedef struct
    {
        uint16_t enc_pin;
        Motor *motor;
    } motor_desc_t;

    void set_torque(float torque);

    Motor(pwm_t pwm, gpio_t pinA, gpio_t pinB, gpio_t enc_pinA, gpio_t enc_pinB, IRQn_Type enc_irq, bool isInv = false);
    ~Motor();
    void set_moode(Motor_Mode mode);
    float get_angle_volocity();
    float get_linear_volocity();

    void set_target_angle_volocity(float speed) { this->target_angle_volocity = speed; };
    void poll(float dt = 0.05, float target_angle_volocity = -12345.54321f);

    pidController &get_pid() { return *pid; };

private:
    pwm_t pwm = {0};
    gpio_t pinA = {0}, pinB = {0}, enc_pinA = {0}, enc_pinB = {0};
    IRQn_Type enc_irq = IRQn_Type(-14);
    bool isInv = false;

    float last_valid_volocity = 0;
    int32_t enc_cnt = 0;

    uint32_t last_intrupt_us = 0;
    uint32_t intrupt_us = 0;
    int8_t direction = 0;

    float target_angle_volocity = 0;
    pidController *pid = new pidController(1.5, 0.9, 0.05, 0.8);

    static motor_desc_t motor_desc_map[MAX_SUPPORT_MOTOR_NUM];

    void handle_enc_a_interrupt();

    friend void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);
};

#endif
